`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date:    14:24:49 06/03/2013 
// Design Name: 
// Module Name:    PmodCON3Controller 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.01 - File Created
// Additional Comments: 
//
//////////////////////////////////////////////////////////////////////////////////
module PmodCON3Controller(
	 input  clk,
	 input reset,
	 input enable,
	 input [3:0] address,
    input [31:0] dataIn,
	 output [31:0] dataOut,
	 input write,
	 output done,
	 output interrupt,
	 input clk_50,
	 output [3:0] servoWires
    );
	 
	 parameter DRP_ID = 32'hA002001A;
	 parameter RELAXED = 7'b1111111;
	 parameter DEFAULT_PERIOD = 11'd2000; // this is the 
	 
	 assign interrupt = 1'b0;
	 
	 reg [6:0] servo1Pos;
	 reg [6:0] servo2Pos;
	 reg [6:0] servo3Pos;
	 reg [6:0] servo4Pos;
	 
	 reg [10:0] PeriodReg = DEFAULT_PERIOD; //initally set the period to the default period
	 
	 reg [31:0] dataOutReg;
	 
	 //returns the current 
	 always @(*) begin
		case(address)
			4'd0: dataOutReg = {1'b0, servo4Pos, 1'b0, servo3Pos, 1'b0, servo2Pos, 1'b0, servo1Pos};
			4'd1: dataOutReg = PeriodReg;
			4'hF: dataOutReg = DRP_ID;
			default: dataOutReg = 32'b0;
		endcase
	 end
	 
	 assign dataOut = dataOutReg;
	 assign done = 1'b1;
	 
	 always @(posedge clk) begin
		servo1Pos <= servo1Pos;
		servo2Pos <= servo2Pos;
		servo3Pos <= servo3Pos;
		servo4Pos <= servo4Pos;
		PeriodReg <= PeriodReg;
		if(reset) begin
			servo1Pos <= RELAXED;
			servo2Pos <= RELAXED;
			servo3Pos <= RELAXED;
			servo4Pos <= RELAXED;
			PeriodReg <= DEFAULT_PERIOD;
		end
		else if(write & enable) begin
			if(address == 4'b0) begin
				servo1Pos <= dataIn[7] ? dataIn[6:0] : servo1Pos;
				servo2Pos <= dataIn[15] ? dataIn[14:8] : servo2Pos;
				servo3Pos <= dataIn[23] ? dataIn[22:16] : servo3Pos;
				servo4Pos <= dataIn[31] ? dataIn[30:24] : servo4Pos;
			end
			else if(address == 4'd1) begin
				PeriodReg <= dataIn[10:0];
			end
		end
	 end
	 
	 reg [8:0] cycleCount;
	 reg [10:0] counter;
	 always @(posedge clk_50) begin
		if(cycleCount == 499) begin // on the 499 cycle (every 0.00001 seconds = 0.01 ms)
			counter <= (counter == (PeriodReg - 11'b1)) ? 11'b0 : counter + 11'b1; //increment the 0.00001 counter and reset to zero if reached period.			cycleCount <= 0; //restart the 0.00001 peroid count
			cycleCount <= 9'b0;
		end
		else begin
			counter <= counter;
			cycleCount <= cycleCount + 9'b1;
		end
	 end
	 
	 wire [7:0]s1stop = 90 + servo1Pos;
	 wire [7:0]s2stop = 90 + servo2Pos;
	 wire [7:0]s3stop = 90 + servo3Pos;
	 wire [7:0]s4stop = 90 + servo4Pos;
	 
	 assign servoWires[0] = (servo1Pos == RELAXED) ? 1'b0 : (counter < s1stop ? 1'b1 : 1'b0);
	 assign servoWires[1] = (servo2Pos == RELAXED) ? 1'b0 : (counter < s2stop ? 1'b1 : 1'b0);
	 assign servoWires[2] = (servo3Pos == RELAXED) ? 1'b0 : (counter < s3stop ? 1'b1 : 1'b0);
	 assign servoWires[3] = (servo4Pos == RELAXED) ? 1'b0 : (counter < s4stop ? 1'b1 : 1'b0);


endmodule
